import numpy as np
from gym_multirotor import utils
from gym_multirotor.envs.mujoco.base_env import UAVBaseEnv

def get_rotor_position(a, b):
    g = np.array([-0.115, -0.09, 0.]) #load center to edge
    p = np.array([qpos[28], qpos[29], qpos[30]]) #load center

    r_0 = np.array([qpos[0], qpos[1], qpos[2]]) #rotor
    r_1 = np.array([qpos[7], qpos[8], qpos[9]])
    r_2 = np.array([qpos[14], qpos[15], qpos[16]])
    r_3 = np.array([qpos[21], qpos[22], qpos[23]])

    l_0 = r_0 - p - g # cable
    l_1 = r_1 - p - g
    l_2 = r_2 - p - g
    l_3 = r_3 - p - g

    # calculate cable unit vector
    length_0 = np.linalg.norm(l_0)
    unit_l_0 = l_0 / length_0



    return np.dot(a_skew, b)